/*
 *  MTouchware_touch.c
 *
 *  This code implements Mtouchware services.
 *
 *  Copyright (C) 2010 LIMSI-CNRS (Orsay-France) and EFREI (Paris-France)
 *
 *  Authors: Rami Ajaj, Nicolas Flasque and Frederic Vernier.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation; either version 2.1 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  $Id$
 */

#include "MTouchware_services.h"
#include <math.h>
#include <stdio.h>

#define STATIONARY_THRESHOLD 0.001

// prototype for local fonctions
////////////////////////////////

void service_stationary(t_applicationData *);
void service_filtremoyen(t_applicationData *);
void service_rawdata(t_applicationData *);
void transmitData(t_applicationData *);
void buildSetMessage(p_touch_info, lo_message *, t_tuio_format);

// code for exported functions
//////////////////////////////

void processData(t_applicationData *appData)
{
    if (appData->appServices & RAW_DATA)
    {
        service_rawdata(appData);
    }
    if (appData->appServices & STATIONARY)
    {
        service_stationary(appData);
    }

    transmitData(appData);
    return;
}

// code for local functions
///////////////////////////

void service_rawdata(t_applicationData *appData)
{
    // copy current input to output, then resend

    int input_idx = appData->input.currentIndex;
    int output_idx = appData->output.currentIndex;

    appData->output.touchdata[output_idx] = appData->input.touchdata[input_idx];

    p_touches in, out;

    in = &(appData->input.touchdata[input_idx]);
    out = &(appData->output.touchdata[output_idx]);

    long nbTouch = in->nbTouchInfo;
    int cpt;

    for(cpt=0; cpt < nbTouch; cpt++)
    {
        p_touch_info ininfo = &(in->touchInfoTab[cpt]);
        p_touch_info outinfo = &(out->touchInfoTab[cpt]);

        if (ininfo->input_status == NOT_IN_CURRENT_BUNDLE)
        {
            outinfo->output_status = DONT_SEND;
        }
        else
        {
            outinfo->output_status = SEND;
        }
    }

    /*output_idx = (output_idx+1) % INPUT_TAB_SIZE;

    appData->output.currentIndex = output_idx;*/ // to be done after transmit only !

    appData->hasToSend = 1;

    return;
}

void service_stationary(t_applicationData *appData)
{
    //printf("service stationary is running\n");

    int input_idx = appData->input.currentIndex;
    int output_idx = appData->output.currentIndex;

    p_touches in, out;

    long nbTouch;
    int cpt;

    int isStationary;

    float distance;
    float curx, cury, curz, oldx, oldy, oldz;

    t_tuio_format f = appData->tuioformat;


    appData->output.touchdata[output_idx] = appData->input.touchdata[input_idx];

    // now check if distance between two_touches is less than some threshold
    // if yes, disable emission (No osc set message for stationary containers)

    in = &(appData->input.touchdata[input_idx]);
    out = &(appData->output.touchdata[output_idx]);

    nbTouch = in->nbTouchInfo;

    for(cpt=0; cpt < nbTouch; cpt++)
    {
        p_touch_info ininfo = &(in->touchInfoTab[cpt]);
        p_touch_info outinfo = &(out->touchInfoTab[cpt]);

       isStationary = 1;

        if (ininfo->input_status == ADDED_NOW)
        {
            isStationary = 0;
        }
        else if (ininfo->input_status == UPDATED_NOW) // search for previous values
        {
            //printf("computing distance\n");

            curx = ininfo->pvalues->where.x;
            cury = ininfo->pvalues->where.y;

            oldx = ininfo->prev->pvalues->where.x;
            oldy = ininfo->prev->pvalues->where.y;

            distance = (curx-oldx)*(curx-oldx);
            distance += (cury-oldy)*(cury-oldy);

            if ((f==TUIO_3D_BLB)||(f==TUIO_3D_CUR)||(f==TUIO_3D_OBJ))
            {
                curz = ininfo->pvalues->where.z;
                oldz = ininfo->pvalues->where.z;
                distance += (curz-oldz)*(curz-oldz);
            }


            distance = sqrt(distance);

            //printf("   distance = %f\n",distance);

            if (distance > STATIONARY_THRESHOLD)
            {
                //printf("moving !\n");
                isStationary = 0;
            }
            else
            {
                //printf("stationary !\n");
            }
        }

        if (isStationary)
        {
            outinfo->output_status = DONT_SEND;
        }
        else
        {
            outinfo->output_status = SEND;
        }
    }

   /* output_idx = (output_idx+1) % INPUT_TAB_SIZE;

    appData->output.currentIndex = output_idx;*/ // to be done after transmit only !! otherwise will transmit empty values (might bug)

    // hasToSend ???

    appData->hasToSend = 1;

    return;
}

void service_filtremoyen(t_applicationData *appData)
{
    return;
}

void transmitData(t_applicationData *appData)
{
    // retrieve current data and post
    if (appData->hasToSend == 1)
    {
        int curIdx = appData->output.currentIndex;
        p_touches tosend = &(appData->output.touchdata[curIdx]);

        curIdx = (curIdx+1)%INPUT_TAB_SIZE;
        appData->output.currentIndex = curIdx;

        // build a sequence :
        // source / alive / set(s) / fseq

        lo_message message;

        // source message

        message = lo_message_new();

        lo_message_add_string(message,"source");
        lo_message_add_string(message,"MTouchware");

        lo_send_message(appData->lo_sender,get_path_string(appData->tuioformat),message);

        lo_message_free(message);

        //  alive list message

        message = lo_message_new();

        lo_message_add_string(message,"alive");

        int cpt;
        int nbTouch = tosend->nbTouchInfo;

        for(cpt=0; cpt < nbTouch; cpt++)
        {
            lo_message_add_int32(message, tosend->alivelist[cpt]);
        }

        lo_send_message(appData->lo_sender,get_path_string(appData->tuioformat),message);
        lo_message_free(message);

        // set messages

        for (cpt=0; cpt < nbTouch; cpt++)
        {
                p_touch_info pinfo = &(tosend->touchInfoTab[cpt]);

                if (pinfo->output_status == SEND)
                {
                    message = lo_message_new();
                    buildSetMessage(pinfo, &message, appData->tuioformat);
                    lo_send_message(appData->lo_sender,get_path_string(appData->tuioformat),message);
                    lo_message_free(message);
                }
        }

        // fseq message

        message = lo_message_new();
        lo_message_add_string(message,"fseq");
        lo_message_add_int32(message,42);

        lo_send_message(appData->lo_sender,get_path_string(appData->tuioformat),message);

        lo_message_free(message);

        appData->hasToSend = 0;
    }
    return;
}

void buildSetMessage(p_touch_info pinfo, lo_message *message, t_tuio_format f)
{

        t_touch_values vals = *(pinfo->pvalues);

        lo_message_add_string(*message,"set");
        lo_message_add_int32(*message,vals.session_id);

        if ((f >= TUIO_2D_OBJ) && (f <= TUIO_3D_OBJ))
        {
                lo_message_add_int32(*message, vals.object_id);
        }

        lo_message_add_float(*message, vals.where.x);
        lo_message_add_float(*message, vals.where.y);

        if ((f!=TUIO_2D_OBJ) && (f!=TUIO_2D_CUR) && (f!=TUIO_2D_BLB))
        {
                lo_message_add_float(*message,vals.where.z);
        }

        if (f==TUIO_2D_CUR)
        {
            lo_message_add_float(*message,vals.speed.sx);
            lo_message_add_float(*message,vals.speed.sy);
            lo_message_add_float(*message,vals.motionAccel);

            // done with 2D_CUR
            return;
        }
        else if ((f==TUIO_25D_CUR) || (f==TUIO_3D_CUR))
        {
            lo_message_add_float(*message,vals.speed.sx);
            lo_message_add_float(*message,vals.speed.sy);
            lo_message_add_float(*message,vals.speed.sz);
            lo_message_add_float(*message,vals.motionAccel);

            // done with 25D and 3D cur
            return;
        }

        // now blobs and objects

        lo_message_add_float(*message, vals.angle.a);

        // angles for 3D blobs & objects
        if ((f==TUIO_3D_OBJ) || (f==TUIO_3D_BLB))
        {
                lo_message_add_float(*message,vals.angle.b);
                lo_message_add_float(*message,vals.angle.c);
        }

        if ((f>= TUIO_2D_BLB) && (f <= TUIO_3D_BLB))
        {
            lo_message_add_float(*message, vals.dims.w);
            lo_message_add_float(*message, vals.dims.h);

            if (f == TUIO_3D_BLB)
            {
                lo_message_add_float(*message, vals.dims.d);
                lo_message_add_float(*message, vals.volume);
            }
            else
            {
                lo_message_add_float(*message, vals.area);
            }
        }

        // add X Y

        lo_message_add_float(*message, vals.speed.sx);
        lo_message_add_float(*message, vals.speed.sy);

        if ((f==TUIO_3D_BLB) || (f==TUIO_3D_OBJ))
        {
            lo_message_add_float(*message, vals.speed.sz);
        }

        lo_message_add_float(*message, vals.rotationVelocity.rvx);

        if ((f==TUIO_3D_BLB) || (f==TUIO_3D_OBJ))
        {
            lo_message_add_float(*message, vals.rotationVelocity.rvy);
            lo_message_add_float(*message, vals.rotationVelocity.rvz);
        }

        lo_message_add_float(*message, vals.motionAccel);
        lo_message_add_float(*message, vals.rotationAccel);

        return;
}
